/*
	cpsystem.cpp

	Author: Chris Haag
	Description:

		Defines body of class that deals with primary management
		of a single physics system in our universe.
*/

#include "cpsystem.h"
#include <memory.h>
#include <math.h>

CPSystem::CPSystem()
{
	/////////////////////////////
	// Programming related
	m_nVertices = 0;
	m_nFaces = 0;
	m_pNext = NULL;
	m_vStatic = NULL;
	m_v = NULL;
	m_f = NULL;
	m_tex = NULL;
	m_tLife = 0;
	m_Type = eOther;

	/////////////////////////////
	// Physics related
	ZeroVertex(m_vCenter);
	ZeroVector(m_vVel);
	ZeroVector(m_vwVel);
	ZeroVector(m_vwp);
	m_matInvI.Identity();
	m_matO.Identity();
	m_Mass = m_OneOverMass = 1; // Default mass (kg)	
}

CPSystem::~CPSystem()
{
	for (int i=0; i < m_nFaces; i++)
	{
		if (m_f[i].pIndices)
			delete m_f[i].pIndices;
	}

	if (m_v) delete m_v;
	if (m_vStatic) delete m_vStatic;
	if (m_f) delete m_f;
	if (m_tex) delete m_tex;
}

void CPSystem::Action(CPSystem* p)
{
	ZeroVector(m_vF);	// Zero the net forces
	ZeroVector(m_vT);	// Zero the torque
}

void CPSystem::Reaction(float t)
{
	_VERTEX* vS = m_vStatic; // Pointer to array of vertices in original form
	_VERTEX* v = m_v;	// Pointer to array of vertices	
	_FACE* f = m_f;		// Pointer to array of faces
	_VECTOR vAcc;		// Acceleration of body
	CMatrix3x3 matO;	// Orientation matrix
	register int i;

	///////////////////////////////////
	// Linear math
	vAcc = m_vF * m_OneOverMass;
	m_vVel += vAcc * t;
	m_vCenter += m_vVel * t;

	///////////////////////////////////
	// Angular math

	// Build the orientation matrix
	matO(0,0) = 0; matO(0,1) = -m_vwVel.z; matO(0,2) = m_vwVel.y;
	matO(1,0) = m_vwVel.z; matO(1,1) = 0; matO(1,2) = -m_vwVel.x;
	matO(2,0) = -m_vwVel.y; matO(2,1) = m_vwVel.x; matO(1,2) = 0;
	// Apply it to the current orientation
	m_matO += matO * m_matO * t;
	// Orthonormalize it
	m_matO.Orthonormalize();
	
	m_vwp += m_vT * t;	// m_vwp = Angular momentum

	// Build the inverse world inertia tensor
	m_matInvWI = m_matO * m_matInvI * m_matO.GetTranspose();

	// Build the angular velocity
	m_vwVel = m_matInvWI * m_vwp; // m_vwVel = Angular velocity

	//////////////////////////////////
	// Now affect the solid

	// Recalculate the vertices
	for (i=0, v = m_v; i < m_nVertices; i++, v++, vS++)
	{
		v->x = vS->x * m_matO(0,0) + vS->y * m_matO(0,1) + vS->z * m_matO(0,2);
		v->y = vS->x * m_matO(1,0) + vS->y * m_matO(1,1) + vS->z * m_matO(1,2);
		v->z = vS->x * m_matO(2,0) + vS->y * m_matO(2,1) + vS->z * m_matO(2,2);

		v->x += m_vCenter.x;
		v->y += m_vCenter.y;
		v->z += m_vCenter.z;
	}
	// ...and the normals

	for (i=0; i < m_nFaces; i++, f++)
	{
		_NORMAL& pNS = f->vStaticNorm;
		f->vNorm.a = pNS.a * m_matO(0,0) + pNS.b * m_matO(0,1) + pNS.c * m_matO(0,2);
		f->vNorm.b = pNS.a * m_matO(1,0) + pNS.b * m_matO(1,1) + pNS.c * m_matO(1,2);
		f->vNorm.c = pNS.a * m_matO(2,0) + pNS.b * m_matO(2,1) + pNS.c * m_matO(2,2);

		v = &m_v[f->pIndices[0]];
		f->vNorm.d = 
			-( f->vNorm.a * v->x + f->vNorm.b * v->y +
			f->vNorm.c * v->z );
	}

	//////////////////////////////
	// Demo-related calculations
	m_tLife += t;
}

//
// p1 = stairs, p2 = cube
//
// or
//
// p1 = cube, p2 = cube
//
unsigned int CPSystem::CollisionDetect(CPSystem* p1, CPSystem* p2)
{
	int i,j,k;
	_VERTEX* v1 = p1->GetVertices();		// Pointer to vertices in object 1	
	_VERTEX* v2 = p2->GetVertices();		// Pointer to vertices in object 2
	int nVertices2 = p2->GetVertexCount();	// Number of vertices in object 2
	int nFaces1 = p1->GetFaceCount();
	int nFaces2 = p2->GetFaceCount();

	/* Do for all vertices in system 2 */
	for (i=0; i < nVertices2; i++, v2++)
	{
		_FACE* f1 = p1->GetFaces();
		unsigned char bInBounds = 1;

		_FACE* COLface = NULL;
		float COLdist = 0;

		/* Do for all faces in system 1 */
		for (j=0; j < nFaces1 && bInBounds; j++, f1++)
		{
			float a = f1->vNorm.a;
			float b = f1->vNorm.b;
			float c = f1->vNorm.c;
			float d = f1->vNorm.d;
			float dist = a*v2->x + b*v2->y + c*v2->z + d;

			/* If the point is "behind" the plane of this face */
			if (dist <= 0 )
			{
				// Lets make sure it's actually in bounds of the face
				_VECTOR vNorm;
				int nIndices1 = f1->nIndices;
				unsigned short* pIndices1 = f1->pIndices;

				vNorm.x = a;
				vNorm.y = b;
				vNorm.z = c;

				/* Do for each bound (edge of face f1) */
				for (k=0; k < nIndices1; k++)
				{
					_VERTEX* vtx1 = &v1[ pIndices1[k] ];
					_VERTEX* vtx2 = &v1[ pIndices1[(k+1) % nIndices1] ];
					_VECTOR v;
					_VECTOR vRes;
					float fTemp;
					float d;
					v.x = vtx1->x - vtx2->x;
					v.y = vtx1->y - vtx2->y;
					v.z = vtx1->z - vtx2->z;

					/* Calculate the distance from the point to this edge */
					Normalize(v);
					vRes = v * vNorm;
					d = -(vtx1->x * vRes.x + vtx1->y * vRes.y + vtx1->z * vRes.z);
					fTemp = v2->x * vRes.x + v2->y * vRes.y + v2->z * vRes.z + d;

					/* If the point is outside the face, break out of the
					loop right now */
					if (fTemp < 0)
						break;
				} // for (k=0; k < nIndices1; k++)

				/* If the vertex is in bounds of the face, and
				the point has penetrated more deeply than the
				currently retained point (if any),
				retain its distance behind the face, and the face. */
				if (k == nIndices1) {
					if (dist > COLdist || !COLface)
					{
						COLdist = dist;
						COLface = f1;
					}
				} // if (k == nIndices1)
				else {
					bInBounds = 0;
				}
			} // if (dist <= 0)
			else {
				bInBounds = 0;
			}
		} // for (j=0; j < nFaces1; j++, f1++)


		/* If v2 has touched or penetrated the system p1 */
		if (bInBounds)
		{
			/* Author's note: I want to push the object onto
			the surface of the colliding body to make it
			more realistic. Comment this all out and you'll
			see for yourself why it -may- be good to do! I only
			say -may- because I'm not a physics genius and this
			may encourage a wrong philosophy?
			
			P.S. This implementation does not use the popular
			method where we 'rewind' in time back to where
			the vertex is in a tolerance range that is OK
			for colliding in. I really ought to do that, but
			this is less math intensive...
			*/

			_FACE* fTmp = p2->GetFaces();
			_VERTEX* vTmp = p2->GetVertices();
			float dx = COLface->vNorm.a * COLdist;
			float dy = COLface->vNorm.b * COLdist;
			float dz = COLface->vNorm.c * COLdist;

			/* Push p2 from below stair to perfect contact
			with stair. We have to move vertices, the center
			of mass, and the normals. */
			for (j=0; j < nVertices2; j++, vTmp++)
			{
				vTmp->x -= dx;
				vTmp->y -= dy;
				vTmp->z -= dz;
			}
			p2->m_vCenter.x -= dx;
			p2->m_vCenter.y -= dy;
			p2->m_vCenter.z -= dz;

			for (j=0; j < nFaces2; j++, fTmp++)
			{
				_NORMAL* pNorm = &fTmp->vNorm;
				pNorm->d += (pNorm->a * dx + pNorm->b * dy + pNorm->c * dz);
			}

			/* Returns non-zero if there was a collision */
			if (Collide(p2, p1, COLface, v2))
				return 1;
		} // if (bInBounds)

	} // for (i=0; i < nVertices2; i++, v2++)
	return 0;
}

/*
	A collision is when a vertex of pSysA penetrates or begins to penetrate
	a face of pSysB. pSysA and pSysB are the rigid bodies involved in the
	collision, f is the face, and pV is the vertex.
*/
unsigned int CPSystem::Collide(CPSystem* pSysA, CPSystem* pSysB, _FACE* f, _VERTEX* pV)
{
	_VECTOR rAP, rBP;	// Vector that goes from the center of mass
						// to the point of collision.

	_VECTOR vAB;		// The difference between the velocities of the
						// two bodies involved in the collision

	_VECTOR vNorm;		// Vector that points in the direction
						// of the normal of the surface (in
						// object B) that collided with a
						// point in object A.

	_VERTEX p = *pV;	// This is the point at which the collision has
						// happened. This point will always be a vertex
						// in object A.
	
	_VECTOR rPerpAP, rPerpBP;	// The cross product between vNorm
								// and rAP and rBP, respectively.

	_VECTOR vVel;		// Actual velocity of point p. Remember, p
						// can be rotating as well as travelling
						// linearly...

	float e = 0.8f;		// Coefficient of restitution
						// 0 = glue
						// 1 = super-elastic 

	vNorm.x = f->vNorm.a;
	vNorm.y = f->vNorm.b;
	vNorm.z = f->vNorm.c;
	rAP = p - pSysA->m_vCenter;
	rBP = p - pSysB->m_vCenter;
	vAB = pSysA->m_vVel - pSysB->m_vVel;

	rPerpAP = rAP * vNorm; // (This is a cross product operation)
	rPerpBP = rBP * vNorm;

	// Calculate the actual velocity of point p.
	vVel = pSysA->m_vVel + (pSysA->m_vwVel * rAP);

	// Find the dot product between the velocity of the
	// point of collision on body A, and the normal of
	// the surface of body B, to see if the vertex is
	// really colliding inward.
	{
		float fDP = DotProduct(vNorm, vVel);

		// if fDP > 0, the point is leaving
		// if fDP = 0, the point is in precise contact with the surface
		// if fDP < 0, the point is penetrating
		if (fDP >= 0) {
			// No velocity change required
			return 0;
		}
	}

	/////////////////////////////////////////////////////////////
	// Ok, it's really colliding. We need to calculate the
	// impulse of collision, which is a vector. The formula I'm
	// using is straight out of Chris Hecker's 4th physics article
	// from his series.

	_VECTOR j;	// j is our impulse vector.(Newtons)

	float fNumer = -(1 + e) * DotProduct(vVel, vNorm);	
	float fDenom = DotProduct(vNorm,vNorm) * (pSysA->m_OneOverMass + pSysB->m_OneOverMass)
		+ DotProduct( 
			((pSysA->m_matInvWI * (rPerpAP * vNorm)) * rPerpAP) +
		    ((pSysB->m_matInvWI * (rPerpBP * vNorm)) * rPerpBP),
			vNorm);
	j = vNorm * (fNumer / fDenom);

	// Now that we have our impulse vector, let's apply it to
	// the rigid body A.
	pSysA->m_vVel += j * pSysA->m_OneOverMass;
	pSysA->m_vwp += rAP * j;

	// Now reverse the direction of j and apply it to rigid
	// body B.
	pSysB->m_vVel += -j * pSysB->m_OneOverMass;
	pSysB->m_vwp += rBP * -j;

	// Emulate friction. This is a hack I came up with
	// in this version of the demo to make it look better.
	// The logic is to degrade angular velocity
	// as a function of in what way body A came into 
	// contact with body B. The damping (damping.cpp) does
	// the rest of the better-looking emulation.
	{
		float fMag = DotProduct(vNorm, j);
		fMag = fMag / Magnitude(j);
		pSysA->m_vwp.x -= pSysA->m_vwp.x * fMag * 0.04f;
		pSysA->m_vwp.y -= pSysA->m_vwp.y * fMag * 0.04f;
		pSysA->m_vwp.z -= pSysA->m_vwp.z * fMag * 0.04f;

		pSysB->m_vwp.x -= pSysB->m_vwp.x * fMag * 0.04f;
		pSysB->m_vwp.y -= pSysB->m_vwp.y * fMag * 0.04f;
		pSysB->m_vwp.z -= pSysB->m_vwp.z * fMag * 0.04f;
	}

	pSysA->m_vwVel = pSysA->m_matInvI * pSysA->m_vwp;
	pSysB->m_vwVel = pSysB->m_matInvI * pSysB->m_vwp;

/*	char sz[256];

	sprintf(sz, "P = %f %f %f\nj = %f %f %f\np = %f %f %f\nw = %f %f %f\n\n",
		p.x, p.y, p.z,
		j.x, j.y, j.z,
		pSysA->m_vwp.x, pSysA->m_vwp.y, pSysA->m_vwp.z,
		pSysA->m_vwVel.x, pSysA->m_vwVel.y, pSysA->m_vwVel.z);

	OutputDebugString(sz);*/
	return 1;
}